
#include "feature/feature_matcher.h"

using namespace feature;

FeatureMatcher::FeatureMatcher(std::shared_ptr<FeatureDatabase> feature_database_ptr) : feature_database_ptr_(feature_database_ptr) {
	window_size_ = 60;
}

void FeatureMatcher::DescriptorMatch(uint64_t& feature_id, const uint8_t& camera_id, const uint64_t& frame_id, const uint64_t& last_frame_id, const std::vector<FeaturePoint>& feature_data) {
	// recovery last frame 
	std::map<uint64_t, FeaturePoint> last_feature_point = feature_database_ptr_->GetOneFrameFeaturePoints(camera_id, last_frame_id);
	// LOG(INFO) << " last_feature_point ===== : " << last_feature_point.size() << std::endl;
	
	std::vector<std::pair<uint64_t, uint64_t>> matched_point_idx;
    std::vector<Vector3d> last_point, curr_point;
	for(const auto& last_feature : last_feature_point) {
		const Vector2d& last_pt = last_feature.second.image_point;
		const auto curr_ptx_lower_boundary = last_pt[0] - window_size_;
        const auto curr_ptx_upper_boundary = last_pt[0] + window_size_;
        const auto curr_pty_lower_boundary = last_pt[1] - window_size_;
        const auto curr_pty_upper_boundary = last_pt[1] + window_size_;
        double best_dist = 1000.0;
        double second_best_dist = 1000.0;
        int64_t best_idx_2 = -1;

        const Vector128d& last_descriptor = last_feature.second.descriptor;

        // LOG(INFO) << " last_descriptor ===== : " << last_descriptor.transpose() << std::endl;

        uint64_t idx_2 = 0;
		for(const auto& curr_feature : feature_data) {
			const Vector2d& curr_pt = curr_feature.image_point;
			++idx_2;
			// discard features who is out of searched window size
            if (curr_pt[0] > curr_ptx_upper_boundary || curr_pt[0] < curr_ptx_lower_boundary || 
            	curr_pt[1] < curr_pty_lower_boundary || curr_pt[1] > curr_pty_upper_boundary) {
                continue;
            }
            const Vector128d& curr_descriptor = curr_feature.descriptor;

            // LOG(INFO) << " curr_descriptor ===== : " << curr_descriptor.transpose() << std::endl;

            double dist = ComputeDescriptorDistance(last_descriptor, curr_descriptor);

            // LOG(INFO) << " dist ===== : " << dist << std::endl;

            if(dist < best_dist) {
            	second_best_dist = best_dist;
                best_dist = dist;
                best_idx_2 = idx_2-1;
            } else if (dist < second_best_dist) {
                second_best_dist = dist;
            }
		}

        // LOG(INFO) << " best_dist ===== : " << best_dist << std::endl;
        // LOG(INFO) << " second_best_dist ===== : " << second_best_dist << std::endl;

        if(best_idx_2 < 0) {
            continue;
        }

        if (best_dist  > 1.0) {
            continue;
        }
        // ratio test
        if (second_best_dist * 0.995 < best_dist) {
            continue;
        }
        
        std::pair<uint64_t, uint64_t> target = std::make_pair(best_idx_2, last_feature.first);
        auto iter = std::find(matched_point_idx.begin(), matched_point_idx.end(), target);
        if (iter != matched_point_idx.end()) { 
            continue;
        }

        // matched_point_idx[last_feature.first] = (uint64_t)best_idx_2; // current_feature id
	    // matched_point_idx.push_back({last_feature.first, (uint64_t)best_idx_2}); // current_feature id
        
        // current feature id and last feature id
        matched_point_idx.push_back({(uint64_t)best_idx_2, last_feature.first}); // current_feature id
        last_point.push_back(last_feature.second.normalized_point);
        curr_point.push_back(feature_data[best_idx_2].normalized_point);
    }
	
	LOG(INFO) << " matched_point_idx ===== : " << matched_point_idx.size() << std::endl;

	if(matched_point_idx.empty()) {
		return;
	}
    std::vector<uchar> inliers;
    FilterByFundamentalMatrix(inliers, last_point, curr_point);

    std::map<uint64_t, uint64_t> inliers_ids;
    for(uint32_t i = 0; i < inliers.size(); ++i) {
        if(!inliers[i]) {
            continue;
        }
        std::pair<uint64_t, uint64_t> id_pair = matched_point_idx[i];
        inliers_ids[id_pair.first] = id_pair.second;
    }

    LOG(INFO) << " inliers_ids ===== : " << inliers_ids.size() << std::endl;
    
    // LOG(INFO) << " feature_id 0===== : " << feature_id << std::endl;

    uint64_t current_idx = 0;
    for(const feature::FeaturePoint& feat_point : feature_data) {
      auto iter_inlier = inliers_ids.find(current_idx);
      if(iter_inlier != inliers_ids.end()) {
         feature_database_ptr_->UpdateFeatureDatabase(camera_id, frame_id, iter_inlier->second, feat_point);
      } else {
         ++feature_id;
         feature_database_ptr_->UpdateFeatureDatabase(camera_id, frame_id, feature_id, feat_point);
      }
      ++current_idx;
    }

    // LOG(INFO) << " feature_id 1===== : " << feature_id << std::endl;

}

void FeatureMatcher::FilterByFundamentalMatrix(std::vector<uchar>& inliers, const std::vector<Vector3d>& last_point, const std::vector<Vector3d>& curr_point) {
    std::vector<cv::Point2f> last_normalized_points;
    std::vector<cv::Point2f> curr_normalized_points;
    for(int i = 0; i < curr_point.size(); ++i) {
        Vector3d last_pt = last_point[i];
        Vector3d curr_pt = curr_point[i];

        last_normalized_points.emplace_back(last_pt[0], last_pt[1]);
        curr_normalized_points.emplace_back(curr_pt[0], curr_pt[1]);
    }

    cv::Mat fundamental_matrix = cv::findFundamentalMat(
        last_normalized_points, curr_normalized_points, 
        cv::FM_RANSAC, 
        3.0,  // param1
        0.99, // param2
        inliers
    );

    (void)fundamental_matrix;
    
    // LOG(INFO) << " fundamental_matrix ===== : " << fundamental_matrix << std::endl;
}
